import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
    # 获取当前package的目录，并以此得到地图文件的文件名
    package_dir = get_package_share_directory('simulator')
    # 设置rviz的加载配置文件位置
    rviz_config = os.path.join(package_dir, 'rviz', '2d.rviz')
    config_lidar = os.path.join(package_dir,'params','lidar_nav2.yaml')
    config_odom_tf = os.path.join(package_dir,'params','odom_tf_nav2.yaml')
    remappings = [('/tf', 'tf'),
                  ('/tf_static', 'tf_static')]
    urdf = os.path.join(package_dir, 'urdf', 'turtlebot3_waffle.urdf')
    with open(urdf, 'r') as infp:
        robot_description = infp.read()
        
    return LaunchDescription([
        # 启动仿真时间节点，在/clock话题中发布时钟
        Node(
           package='simulator',
           executable='clock_node',
           name='clock',
           output='screen'
           ),
        # 启动urdf
        Node(
           package='robot_state_publisher',
           executable='robot_state_publisher',
           name='robot_state_publisher',
           output='screen',
           parameters=[{'use_sim_time': True,
                     'robot_description': robot_description}],
           remappings=remappings),
        #启动里程计
        Node(
           package='simulator',
           executable='odom_tf',
           name='odom_tf',
           parameters=[{'use_sim_time': True},
                       config_odom_tf]),    
        #Node(
        #     package='tf2_ros',
        #     executable='static_transform_publisher',
        #     name='map_to_laser_link',
        #     arguments=[
        #         '--x', '0', '--y', '0', '--z', '0',
        #         '--yaw', '0', '--pitch', '0', '--roll', '0',
        #         '--frame-id', 'base_link', '--child-frame-id', 'laser_link'
        #     ],
        #     parameters=[{}],),
        # 打开一个用于显示的rviz2终端
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config],
            parameters=[{'use_sim_time': True}],
            ),
        # simulator Lidar
        Node(
            package='simulator',
            executable='simulator_lidar',
            name='simulator_lidar',
            output='screen',
            parameters=[{'use_sim_time': True},config_lidar]
        )
    ])
